#pragma once

#ifndef _APP_GIMBAL_H_
#define _APP_GIMBAL_H_

#include "SRML.h"
#include <stdint.h>
#include "drv_can.h"
#include "PID.h"
#include "App_MotorCtrl.h"
#include "PID.h"
#include <UpperMonitor.h>

#ifdef __cplusplus
extern "C" {
#endif
	
#include "stm32f4xx_hal.h"

union transmit_u
{
  float     v[3];
	uint8_t   out[12];
};

#ifdef __cplusplus
}
#endif

typedef struct  GlobalCoordinate
{
	float vx; 
	float	vy;
	float	az;
}GlobalC;

typedef struct ChassisCoordinate
{
	float Wheel1; 
	float	Wheel2;
	float	Wheel3;
}ChassisC;

typedef struct AngleYawAndCosSin
{
	float yaw; 
	float	sin_theta;
	float	cos_theta;
}AngleY;


void Chassis_Init(void);
uint32_t Chassis_Receive_HERO(uint8_t *,uint16_t);

void Chassis_Receive_Motor(CAN_RxBuffer* );
void Chassis_Transmit_HERO(UART_HandleTypeDef* );
void Chassis_Adjust(void);

void Chassis_World_Angle(void);
void Chassis_AngleCal(AngleY *);
void Chassis_Cal(GlobalC *, ChassisC *, AngleY *);
void Chassis_arCal(ChassisC *,GlobalC *, AngleY *);

#endif 
